#!/usr/bin/env python3
# -*- coding: utf-8 -*-

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # 获取包路径
    pkg_dir = get_package_share_directory('fruit_cam')
    
    # 声明参数
    camera_name = LaunchConfiguration('camera_name', default='left_camera')
    camera_device = LaunchConfiguration('camera_device', default='/dev/v4l/by-path/pci-0000:00:14.0-usb-0:2:1.0-video-index0')
    camera_frame_id = LaunchConfiguration('camera_frame_id', default='camera_optical_frame')
    
    # 性能相关参数
    buffer_size = LaunchConfiguration('buffer_size', default='5')
    io_method = LaunchConfiguration('io_method', default='mmap')  # 使用mmap可以提高性能
    
    # 相机信息路径
    camera_info_path = os.path.join(pkg_dir, 'config', 'left_cam.yaml')
    camera_info_url = LaunchConfiguration(
        'camera_info_url', 
        default=f'file://{camera_info_path}'
    )
    
    # 创建参数声明
    declare_camera_name = DeclareLaunchArgument(
        'camera_name',
        default_value='left_camera',
        description='摄像头名称'
    )
    
    declare_camera_device = DeclareLaunchArgument(
        'camera_device',
        default_value='/dev/v4l/by-path/pci-0000:00:14.0-usb-0:2:1.0-video-index0',
        description='摄像头设备路径'
    )
    
    declare_camera_frame_id = DeclareLaunchArgument(
        'camera_frame_id',
        default_value='camera_optical_frame',
        description='摄像头坐标系ID'
    )
    
    declare_camera_info_url = DeclareLaunchArgument(
        'camera_info_url',
        default_value=f'file://{camera_info_path}',
        description='相机标定文件URL'
    )
    
    # 性能相关参数声明
    declare_buffer_size = DeclareLaunchArgument(
        'buffer_size',
        default_value='5',
        description='相机缓冲区大小'
    )
    
    declare_io_method = DeclareLaunchArgument(
        'io_method',
        default_value='mmap',
        description='相机IO方法 (mmap/read/userptr)'
    )
    
    # 创建v4l2_camera节点
    v4l2_camera_node = Node(
        package='v4l2_camera',
        executable='v4l2_camera_node',
        name='left_v4l2_camera',
        parameters=[{
            'camera_name': camera_name,
            'video_device': '/dev/video0',  # 直接硬编码设备路径进行测试
            'frame_id': camera_frame_id,
            'pixel_format': 'YUYV',  # 使用相机支持的YUYV格式
            'output_encoding': 'rgb8',  # 输出为RGB8格式
            'image_size': [640, 480],  # 降低分辨率以提高帧率
            'camera_info_url': camera_info_url,
            'publish_rate': 15.0,  # 降低发布频率以减少资源占用
            'auto_exposure': 1,  # 设置为手动曝光
            'exposure_time_absolute': 100,  # 降低曝光时间
            'buffer_size': buffer_size,  # 增加缓冲区大小
            'io_method': io_method,  # 使用mmap方法
            'use_v4l2_buffer_timestamps': True,  # 使用V4L2缓冲区时间戳
            'processing_threads': 1  # 减少处理线程数以降低资源占用
        }],
        remappings=[
            ('image_raw', '/left_cam/image_raw'),
            ('camera_info', '/left_cam/camera_info'),
        ],
        output='screen',
    )
    
    # 创建帧率监控节点
    framerate_monitor_node = Node(
        package='fruit_cam',
        executable='framerate_monitor',
        name='left_framerate_monitor',
        parameters=[{
            'image_topic': '/left_cam/image_raw',
        }],
        output='screen',
    )
    
    return LaunchDescription([
        # 参数声明
        declare_camera_name,
        declare_camera_device,
        declare_camera_frame_id,
        declare_camera_info_url,
        declare_buffer_size,
        declare_io_method,
        
        # 启动节点
        v4l2_camera_node,
        framerate_monitor_node,
    ])